#!/usr/bin/env python
#coding=utf-8

import sys
sys.path.append('../')
import wx
import wxui.hostUi
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
import threading
import time



class UiHandler(wxui.hostUi.MainFrame, threading.Thread):
    # global controller_pub, cmd_vel_pub
    def __init__(self, parent):
        threading.Thread.__init__(self)
        wxui.hostUi.MainFrame.__init__(self, parent)

        rospy.init_node('host_contrller_node', anonymous=True)
        self.controller_pub = rospy.Publisher('controller', String, queue_size=10)
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
        self.mov_cmd = Twist()
        self.start()
        self.stop_cmd = Twist()
        self.stop_cmd.linear.x = 0
        self.stop_cmd.angular.z = 0

    def onForwardClicked(self, event):
        print ('forward')
        mov_cmd = Twist()
        mov_cmd.linear.x = 1.0
        mov_cmd.linear.y = 0
        mov_cmd.angular.z = 0.5
        self.cmd_vel_pub.publish(mov_cmd)
        self.mov_cmd = mov_cmd
    def onForwardUp( self, event ):
        self.mov_cmd = self.stop_cmd

    def onBackwardClicked(self, event):
        print ('backward')
        mov_cmd = Twist()
        mov_cmd.linear.x = -1.0
        mov_cmd.linear.y = 0
        mov_cmd.angular.z = -0.5
        self.cmd_vel_pub.publish(mov_cmd)
        self.mov_cmd = mov_cmd
    def onBackwardUp( self, event ):
        self.mov_cmd = self.stop_cmd

    def onLeftClicked(self, event):
        print ('Left')
        mov_cmd = Twist()
        mov_cmd.linear.x = 0.5
        mov_cmd.linear.y = 0
        mov_cmd.angular.z = -1
        self.cmd_vel_pub.publish(mov_cmd)
        self.mov_cmd = mov_cmd
    def onLeftUp( self, event ):
        self.mov_cmd = self.stop_cmd

    def onRightClicked(self, event):
        print ('Right')
        mov_cmd = Twist()
        mov_cmd.linear.x = 1
        mov_cmd.linear.y = 0
        mov_cmd.angular.z = 1
        self.cmd_vel_pub.publish(mov_cmd)
        self.mov_cmd = mov_cmd
    def onRightUp( self, event ):
        self.mov_cmd = self.stop_cmd

    def onManualStart(self, event):
        print ('Manual start')
        self.controller_pub.publish('Manual start')
    def onManualStop(self, event):
        print ('Manual stop')
        self.controller_pub.publish('Manuan stop')
    def onAutoStart(self, event):
        print ('Auto start')
        self.controller_pub.publish('Auto start')
    def onAutoStop(self, event):
        print ('Auto stop')
        self.controller_pub.publish('Auto stop')

    def onMapStart(self, event):
        print ('Map start')
        self.controller_pub.publish('Map start')
    def onMapStop(self, event):
        print ('Map stop')
        self.controller_pub.publish('Map stop')


    def run(self):
        while(True):
            self.cmd_vel_pub.publish(self.mov_cmd)
            time.sleep(1)
            print("mov_cmd:{}\r\n", self.mov_cmd.linear.x)

def Init():
    # global controller_pub, cmd_vel_pub
    # controller_pub = rospy.Publisher('controller', String, queue_size=10)
    # controller_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
    app = wx.App()
    frame = UiHandler(None)
    frame.Show()
    app.MainLoop()
# app = wx.App()
# win = wx.Frame(None,-1,title='机器人控制系统', size=(400,300))
# panel = wx.Panel(win)
# label = wx.StaticText(panel, label="helloworld", pos=(100,100))
# btn = wx.Button(panel, label = 'Button')
# win.Show()
# app.MainLoop()